/*
 *  Matrix4.cpp
 *  zray-gui
 *
 *  Created by czalkin on 1/10/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "Matrix4.hpp"
#include "Vector3.hpp"
#include "Vector4.hpp"
#include "Point3.hpp"

#include <cmath>
#include <cassert>

Matrix4::Matrix4()
{
	v[0] = 1.0;
	v[1] = 0.0;
	v[2] = 0.0f;
	v[3] = 0.0f;
	
	v[4] = 0.0f;
	v[5] = 1.0f;
	v[6] = 0.0f;
	v[7] = 0.0f;
	
	v[8] = 0.0f;
	v[9] = 0.0f;
	v[10] = 1.0f;
	v[11] = 0.0f;
	
	v[12] = 0.0f;
	v[13] = 0.0f;
	v[14] = 0.0f;
	v[15] = 1.0f;
}

Matrix4::Matrix4( float x0, float x1, float x2, float x3,
				  float y0, float y1, float y2, float y3,
				  float z0, float z1, float z2, float z3,
				  float t0, float t1, float t2, float t3 )
{
	v[0] = x0;
	v[1] = x1;
	v[2] = x2;
	v[3] = x3;
	
	v[4] = y0;
	v[5] = y1;
	v[6] = y2;
	v[7] = y3;
	
	v[8] = z0;
	v[9] = z1;
	v[10] = z2;
	v[11] = z3;
	
	v[12] = t0;
	v[13] = t1;
	v[14] = t2;
	v[15] = t3;
}

Matrix4::Matrix4( float* col1, float *col2, float *col3, float* col4 )
{
	for( int i=0; i<4; ++i )
	{
		v[i] = col1[i];
		v[4+i] = col2[i];
		v[8+i] = col3[i];
		v[12+i] = col4[i];
	}
}

Matrix4::Matrix4( const Vector3 &x, const Vector3 &y, const Vector3 &z )
{
	v[0] = x[0];
	v[1] = x[1];
	v[2] = x[2];
	v[3] = 0;
	v[4] = y[0];
	v[5] = y[1];
	v[6] = y[2];
	v[7] = 0;
	v[8] = z[0];
	v[9] = z[1];
	v[10] = z[2];
	v[11] = 0;
	v[12] = 0;
	v[13] = 0;
	v[14] = 0;
	v[15] = 1;
}

Matrix4::Matrix4( const Vector3 &x, const Vector3 &y, const Vector3 &z, const Vector3 &t )
{
	v[0] = x[0];
	v[1] = x[1];
	v[2] = x[2];
	v[3] = 0;
	v[4] = y[0];
	v[5] = y[1];
	v[6] = y[2];
	v[7] = 0;
	v[8] = z[0];
	v[9] = z[1];
	v[10] = z[2];
	v[11] = 0;
	v[12] = t[0];
	v[13] = t[1];
	v[14] = t[2];
	v[15] = 1;
}


Matrix4::Matrix4( float* all )
{
	for( int i=0; i<16;++i )
		v[i] = all[i];
}


Matrix4::Matrix4( const Matrix4 &rhs )
{
	for( int i=0; i<16;++i )
		v[i] = rhs.v[i];
}


Matrix4 Matrix4::getRotation() const
{
	return Matrix4( v[0], v[1], v[2],  v[3],
				    v[4], v[5], v[6],  v[7],
				    v[8], v[9], v[10], v[11],
				    0,    0,    0,     1 );
}

Matrix4& Matrix4::operator*=( const Matrix4 &rhs )
{
	Matrix4 out;
	out.v[0 ] = v[0 ]*rhs.v[0 ] + v[4 ]*rhs.v[1 ] + v[8 ]*rhs.v[2 ] + v[12]*rhs.v[3 ];
	out.v[1 ] = v[1 ]*rhs.v[0 ] + v[5 ]*rhs.v[1 ] + v[9 ]*rhs.v[2 ] + v[13]*rhs.v[3 ];
	out.v[2 ] = v[2 ]*rhs.v[0 ] + v[6 ]*rhs.v[1 ] + v[10]*rhs.v[2 ] + v[14]*rhs.v[3 ];
	out.v[3 ] = v[3 ]*rhs.v[0 ] + v[7 ]*rhs.v[1 ] + v[11]*rhs.v[2 ] + v[15]*rhs.v[3 ];
	
	out.v[4 ] = v[0 ]*rhs.v[4 ] + v[4 ]*rhs.v[5 ] + v[8 ]*rhs.v[6 ] + v[12]*rhs.v[7 ];
	out.v[5 ] = v[1 ]*rhs.v[4 ] + v[5 ]*rhs.v[5 ] + v[9 ]*rhs.v[6 ] + v[13]*rhs.v[7 ];
	out.v[6 ] = v[2 ]*rhs.v[4 ] + v[6 ]*rhs.v[5 ] + v[10]*rhs.v[6 ] + v[14]*rhs.v[7 ];
	out.v[7 ] = v[3 ]*rhs.v[4 ] + v[7 ]*rhs.v[5 ] + v[11]*rhs.v[6 ] + v[15]*rhs.v[7 ];
	
	out.v[8 ] = v[0 ]*rhs.v[8 ] + v[4 ]*rhs.v[9 ] + v[8 ]*rhs.v[10] + v[12]*rhs.v[11];
	out.v[9 ] = v[1 ]*rhs.v[8 ] + v[5 ]*rhs.v[9 ] + v[9 ]*rhs.v[10] + v[13]*rhs.v[11];
	out.v[10] = v[2 ]*rhs.v[8 ] + v[6 ]*rhs.v[9 ] + v[10]*rhs.v[10] + v[14]*rhs.v[11];
	out.v[11] = v[3 ]*rhs.v[8 ] + v[7 ]*rhs.v[9 ] + v[11]*rhs.v[10] + v[15]*rhs.v[11];
	
	out.v[12] = v[0 ]*rhs.v[12] + v[4 ]*rhs.v[13] + v[8 ]*rhs.v[14] + v[12]*rhs.v[15];
	out.v[13] = v[1 ]*rhs.v[12] + v[5 ]*rhs.v[13] + v[9 ]*rhs.v[14] + v[13]*rhs.v[15];
	out.v[14] = v[2 ]*rhs.v[12] + v[6 ]*rhs.v[13] + v[10]*rhs.v[14] + v[14]*rhs.v[15];
	out.v[15] = v[3 ]*rhs.v[12] + v[7 ]*rhs.v[13] + v[11]*rhs.v[14] + v[15]*rhs.v[15];	
	
	for( unsigned int i=0; i<16; ++i )
	{
		v[i] = out.v[i];
	}
	
	
	return *this;
}

Matrix4 Matrix4::operator*( const Matrix4 &rhs ) const
{
	Matrix4 out;
	out.v[0 ] = v[0 ]*rhs.v[0 ] + v[4 ]*rhs.v[1 ] + v[8 ]*rhs.v[2 ] + v[12]*rhs.v[3 ];
	out.v[1 ] = v[1 ]*rhs.v[0 ] + v[5 ]*rhs.v[1 ] + v[9 ]*rhs.v[2 ] + v[13]*rhs.v[3 ];
	out.v[2 ] = v[2 ]*rhs.v[0 ] + v[6 ]*rhs.v[1 ] + v[10]*rhs.v[2 ] + v[14]*rhs.v[3 ];
	out.v[3 ] = v[3 ]*rhs.v[0 ] + v[7 ]*rhs.v[1 ] + v[11]*rhs.v[2 ] + v[15]*rhs.v[3 ];

	out.v[4 ] = v[0 ]*rhs.v[4 ] + v[4 ]*rhs.v[5 ] + v[8 ]*rhs.v[6 ] + v[12]*rhs.v[7 ];
	out.v[5 ] = v[1 ]*rhs.v[4 ] + v[5 ]*rhs.v[5 ] + v[9 ]*rhs.v[6 ] + v[13]*rhs.v[7 ];
	out.v[6 ] = v[2 ]*rhs.v[4 ] + v[6 ]*rhs.v[5 ] + v[10]*rhs.v[6 ] + v[14]*rhs.v[7 ];
	out.v[7 ] = v[3 ]*rhs.v[4 ] + v[7 ]*rhs.v[5 ] + v[11]*rhs.v[6 ] + v[15]*rhs.v[7 ];
	
	out.v[8 ] = v[0 ]*rhs.v[8 ] + v[4 ]*rhs.v[9 ] + v[8 ]*rhs.v[10] + v[12]*rhs.v[11];
	out.v[9 ] = v[1 ]*rhs.v[8 ] + v[5 ]*rhs.v[9 ] + v[9 ]*rhs.v[10] + v[13]*rhs.v[11];
	out.v[10] = v[2 ]*rhs.v[8 ] + v[6 ]*rhs.v[9 ] + v[10]*rhs.v[10] + v[14]*rhs.v[11];
	out.v[11] = v[3 ]*rhs.v[8 ] + v[7 ]*rhs.v[9 ] + v[11]*rhs.v[10] + v[15]*rhs.v[11];
	
	out.v[12] = v[0 ]*rhs.v[12] + v[4 ]*rhs.v[13] + v[8 ]*rhs.v[14] + v[12]*rhs.v[15];
	out.v[13] = v[1 ]*rhs.v[12] + v[5 ]*rhs.v[13] + v[9 ]*rhs.v[14] + v[13]*rhs.v[15];
	out.v[14] = v[2 ]*rhs.v[12] + v[6 ]*rhs.v[13] + v[10]*rhs.v[14] + v[14]*rhs.v[15];
	out.v[15] = v[3 ]*rhs.v[12] + v[7 ]*rhs.v[13] + v[11]*rhs.v[14] + v[15]*rhs.v[15];	
	return out;
}

float Matrix4::at( int col, int row )
{
	return v[ 4*col + row ];
}

void Matrix4::set( int col, int row, float val )
{
	v[ 4*col + row ] = val;
}


Matrix4 Matrix4::viewport_matrix( const float width, const float height)
{	
	float frame = width / height;
	float screen[] = { -frame, frame, -1, 1 };

	Matrix4 screenToRaster = Matrix4::createScale( width, height, 1 );
	screenToRaster = screenToRaster * Matrix4::createScale( 1.0/(screen[1] - screen[0]), 1.0/(screen[2] - screen[3]), 1.0f );
	screenToRaster = screenToRaster * Matrix4::createTranslate( -screen[2], -screen[3], 0.f );
	Matrix4 rasterToScreen = screenToRaster.invert();
	return rasterToScreen;
}

Matrix4 Matrix4::lookat_matrix( Vector3 eye, Vector3 at, Vector3 up, bool doTranslation )
{
	Matrix4 m;

	
	Vector3 f = at - eye;
	f.normalize_inPlace();
	
	up.normalize_inPlace();

	Vector3 s = f.cross( up );
	s.normalize_inPlace();
		
	Vector3 u = s.cross( f );
	u.normalize_inPlace();
	
	m.v[0] = s[0];
	m.v[1] = s[1];
	m.v[2] = s[2];
	m.v[3] = 0.0f;
	
	m.v[4] = u[0];
	m.v[5] = u[1];
	m.v[6] = u[2];
	m.v[7] = 0.0f;
	
	m.v[8] =  -f[0];
	m.v[9] =  -f[1];
	m.v[10] = -f[2];
	m.v[11] = 0.0f;

	m.v[12] = 0;
	m.v[13] = 0;
	m.v[14] = 0;
	m.v[15] = 1.0f;
	
	if( doTranslation )
	{
		Matrix4 trans = createTranslate( -eye[0], -eye[1], -eye[2] );
		return trans*m;
	} else {
		return m;
	}
}

Matrix4 Matrix4::scale( float scale )
{
	for( unsigned int i=0; i<16; ++i )
		v[i] *= scale;
	
	return *this;
}


Matrix4 Matrix4::invert()
{
	Matrix4 out;	
	out.v[0] = v[9]*v[14]*v[7] - v[13]*v[10]*v[7] + v[13]*v[6]*v[11] - v[5]*v[14]*v[11] - v[9]*v[6]*v[15] + v[5]*v[10]*v[15];
	out.v[4] = v[12]*v[10]*v[7] - v[8]*v[14]*v[7] - v[12]*v[6]*v[11] + v[4]*v[14]*v[11] + v[8]*v[6]*v[15] - v[4]*v[10]*v[15];
	out.v[8] = v[8]*v[13]*v[7] - v[12]*v[9]*v[7] + v[12]*v[5]*v[11] - v[4]*v[13]*v[11] - v[8]*v[5]*v[15] + v[4]*v[9]*v[15];
	out.v[12] = v[12]*v[9]*v[6] - v[8]*v[13]*v[6] - v[12]*v[5]*v[10] + v[4]*v[13]*v[10] + v[8]*v[5]*v[14] - v[4]*v[9]*v[14];
	out.v[1] = v[13]*v[10]*v[3] - v[9]*v[14]*v[3] - v[13]*v[2]*v[11] + v[1]*v[14]*v[11] + v[9]*v[2]*v[15] - v[1]*v[10]*v[15];
	out.v[5] = v[8]*v[14]*v[3] - v[12]*v[10]*v[3] + v[12]*v[2]*v[11] - v[0]*v[14]*v[11] - v[8]*v[2]*v[15] + v[0]*v[10]*v[15];
	out.v[9] = v[12]*v[9]*v[3] - v[8]*v[13]*v[3] - v[12]*v[1]*v[11] + v[0]*v[13]*v[11] + v[8]*v[1]*v[15] - v[0]*v[9]*v[15];
	out.v[13] = v[8]*v[13]*v[2] - v[12]*v[9]*v[2] + v[12]*v[1]*v[10] - v[0]*v[13]*v[10] - v[8]*v[1]*v[14] + v[0]*v[9]*v[14];
	out.v[2] = v[5]*v[14]*v[3] - v[13]*v[6]*v[3] + v[13]*v[2]*v[7] - v[1]*v[14]*v[7] - v[5]*v[2]*v[15] + v[1]*v[6]*v[15];
	out.v[6] = v[12]*v[6]*v[3] - v[4]*v[14]*v[3] - v[12]*v[2]*v[7] + v[0]*v[14]*v[7] + v[4]*v[2]*v[15] - v[0]*v[6]*v[15];
	out.v[10] = v[4]*v[13]*v[3] - v[12]*v[5]*v[3] + v[12]*v[1]*v[7] - v[0]*v[13]*v[7] - v[4]*v[1]*v[15] + v[0]*v[5]*v[15];
	out.v[14] = v[12]*v[5]*v[2] - v[4]*v[13]*v[2] - v[12]*v[1]*v[6] + v[0]*v[13]*v[6] + v[4]*v[1]*v[14] - v[0]*v[5]*v[14];
	out.v[3] = v[9]*v[6]*v[3] - v[5]*v[10]*v[3] - v[9]*v[2]*v[7] + v[1]*v[10]*v[7] + v[5]*v[2]*v[11] - v[1]*v[6]*v[11];
	out.v[7] = v[4]*v[10]*v[3] - v[8]*v[6]*v[3] + v[8]*v[2]*v[7] - v[0]*v[10]*v[7] - v[4]*v[2]*v[11] + v[0]*v[6]*v[11];
	out.v[11] = v[8]*v[5]*v[3] - v[4]*v[9]*v[3] - v[8]*v[1]*v[7] + v[0]*v[9]*v[7] + v[4]*v[1]*v[11] - v[0]*v[5]*v[11];
	out.v[15] = v[4]*v[9]*v[2] - v[8]*v[5]*v[2] + v[8]*v[1]*v[6] - v[0]*v[9]*v[6] - v[4]*v[1]*v[10] + v[0]*v[5]*v[10];
	out.scale(1/determinant());
	return out;
}
	
	
	
float Matrix4::determinant()
{
	float value =
	v[12] * v[9] * v[6] * v[3]-v[8] * v[13] * v[6] * v[3]-v[12] * v[5] * v[10] * v[3]+v[4] * v[13]    * v[10] * v[3]+
	v[8] * v[5] * v[14] * v[3]-v[4] * v[9] * v[14] * v[3]-v[12] * v[9] * v[2] * v[7]+v[8] * v[13]    * v[2] * v[7]+
	v[12] * v[1] * v[10] * v[7]-v[0] * v[13] * v[10] * v[7]-v[8] * v[1] * v[14] * v[7]+v[0] * v[9]    * v[14] * v[7]+
	v[12] * v[5] * v[2] * v[11]-v[4] * v[13] * v[2] * v[11]-v[12] * v[1] * v[6] * v[11]+v[0] * v[13]    * v[6] * v[11]+
	v[4] * v[1] * v[14] * v[11]-v[0] * v[5] * v[14] * v[11]-v[8] * v[5] * v[2] * v[15]+v[4] * v[9]    * v[2] * v[15]+
	v[8] * v[1] * v[6] * v[15]-v[0] * v[9] * v[6] * v[15]-v[4] * v[1] * v[10] * v[15]+v[0] * v[5]    * v[10] * v[15];
	return value;
}


Vector4 Matrix4::operator*( const Vector4 &vec ) const
{
	Vector4 out;
	out.v[0] = v[0]*vec.v[0] + v[4]*vec[1] + v[8]*vec.v[2] + v[12]*vec.v[3];
	out.v[1] = v[1]*vec.v[0] + v[5]*vec[1] + v[9]*vec.v[2] + v[13]*vec.v[3];
	out.v[2] = v[2]*vec.v[0] + v[6]*vec[1] + v[10]*vec.v[2] + v[14]*vec.v[3];
	out.v[3] = v[3]*vec.v[0] + v[7]*vec[1] + v[11]*vec.v[2] + v[15]*vec.v[3];
	return out;
}

Vector3 Matrix4::operator*( const Vector3 &vecIn ) const
{
	Vector4 vec( vecIn, 0.0f );
	Vector4 out;
	out.v[0] = v[0]*vec.v[0] + v[4]*vec.v[1] + v[8]*vec.v[2] + v[12]*vec.v[3];
	out.v[1] = v[1]*vec.v[0] + v[5]*vec.v[1] + v[9]*vec.v[2] + v[13]*vec.v[3];
	out.v[2] = v[2]*vec.v[0] + v[6]*vec.v[1] + v[10]*vec.v[2] + v[14]*vec.v[3];
	out.v[3] = v[3]*vec.v[0] + v[7]*vec.v[1] + v[11]*vec.v[2] + v[15]*vec.v[3];
	return out.homogeneous();
}

/// Transform a point
Vector3 Matrix4::mult34( const Vector3 &vecIn ) const
{
	Vector4 out;
	out.v[0] = v[0]*vecIn.v[0] + v[4]*vecIn.v[1] + v[8]*vecIn.v[2] + v[12];
	out.v[1] = v[1]*vecIn.v[0] + v[5]*vecIn.v[1] + v[9]*vecIn.v[2] + v[13];
	out.v[2] = v[2]*vecIn.v[0] + v[6]*vecIn.v[1] + v[10]*vecIn.v[2] + v[14];
	out.v[3] = v[3]*vecIn.v[0] + v[7]*vecIn.v[1] + v[11]*vecIn.v[2] + v[15];
	return out.homogeneous();
}

/// Transform a point
Point3 Matrix4::mult34( const Point3 &in ) const
{
	Vector4 out;
	out.v[0] = v[0]*in.v[0] + v[4]*in.v[1] + v[8]*in.v[2] + v[12];
	out.v[1] = v[1]*in.v[0] + v[5]*in.v[1] + v[9]*in.v[2] + v[13];
	out.v[2] = v[2]*in.v[0] + v[6]*in.v[1] + v[10]*in.v[2] + v[14];
	out.v[3] = v[3]*in.v[0] + v[7]*in.v[1] + v[11]*in.v[2] + v[15];
	return out.homogeneous();
}


/// Transform a vector

Vector3 Matrix4::mult33( const Vector3 &vecIn ) const
{
	Vector3 out;
	out.v[0] = v[0]*vecIn.v[0] + v[4]*vecIn.v[1] + v[8]*vecIn.v[2];
	out.v[1] = v[1]*vecIn.v[0] + v[5]*vecIn.v[1] + v[9]*vecIn.v[2];
	out.v[2] = v[2]*vecIn.v[0] + v[6]*vecIn.v[1] + v[10]*vecIn.v[2];
	return out;
}

Matrix4 Matrix4::createRotationX( const float deg )
{
	float rad = deg / 180.0f * 3.1415926535f;
	float s = sin( rad );
	float c = cos( rad );
	
	return Matrix4( 1, 0, 0, 0,   0, c, s, 0,   0, -s, c, 0,   0, 0, 0, 1 );
}

Matrix4 Matrix4::createRotationY( const float deg )
{
	float rad = deg / 180.0f * 3.1415926535f;
	float s = sin( rad );
	float c = cos( rad );
	
	return Matrix4( c, 0, -s, 0,    0, 1, 0, 0,     s, 0, c, 0,     0, 0, 0, 1 );
}

Matrix4 Matrix4::createRotationZ( const float deg )
{
	float rad = deg / 180.0f * 3.1415926535f;
	float s = sin( rad );
	float c = cos( rad );
	
	return Matrix4( c, 0, -s, 0,   -s, c, 0, 0,   0, 0, 1, 0,   0, 0, 0, 1 );
}

const Vector3 Matrix4::getTranslation() const
{
	return Vector3( v[12], v[13], v[14] );
}

